﻿using System;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;

namespace Assets.Scripts.LDS
{
    static class LayoutPlanner
    {
        public static Transform toolman;
        public static Transform pointsLayout;

        public static SceneSemGraph ssg;

        public static Dictionary<string, List<Vector3>> origin_planes = new Dictionary<string, List<Vector3>>();
        public static Dictionary<string, List<Vector3>> origin_support = new Dictionary<string, List<Vector3>>();

        private static HashSet<int> adjustedModelIdxs = new HashSet<int>();

        private static List<string> blackListForPlanes = new List<string> {
    "Picture1", "Picture2", "sculpture1", "sculpture2",
    "drums", "stone_lod0", "blackboard", "chalk", "gallery", "window",
    "shutter", "MasterBedroom-2414-mesh", "LivingDiningRoom-6151-mesh", "Library-12973_mesh"
};

        private static (List<Vector3>, List<List<Vector3>>) loadPlane(string id)
        {
            List<Vector3> bounding_box = new List<Vector3>();
            List<List<Vector3>> support_planes = new List<List<Vector3>>();
            using (StreamReader sr = new StreamReader(Params.modelPlane_dir + id + ".txt"))
            {
                // bounding_box
                string line = sr.ReadLine();
                if (!line.Contains("bounding_box"))
                {
                    Debug.LogError("ModelPlanes.txt format error: " + Params.modelPlane_dir + id + ".txt");
                }

                string[] values = line.Split('|');
                for (int i = 0; i < values.Length; i++)
                    values[i] = values[i].Trim();

                if ((values.Length != 10 && values.Length != 9) || !values[0].Equals("bounding_box"))
                    Debug.LogError("ModelPlanes.txt format error: " + Params.modelPlane_dir + id + ".txt");

                for (int i = 1; i <= 8; i++)
                {
                    string[] vectors = values[i].Split(',');
                    bounding_box.Add(new Vector3(float.Parse(vectors[0]), float.Parse(vectors[1]), float.Parse(vectors[2])));
                }

                // support_planes
                line = sr.ReadLine();
                if (!line.Contains("support_planes"))
                    Debug.LogError("ModelPlanes.txt format error");
                int count = Int32.Parse(line.Split(' ')[1]);

                for (int i = 0; i < count; i++)
                {
                    line = sr.ReadLine();
                    List<Vector3> support_planes_points = new List<Vector3>();
                    values = line.Split('|');

                    if (values.Length != 4 && values.Length != 5)
                        Debug.LogError("ModelPlanes.txt format error");

                    for (int j = 0; j < 4; j++)
                    {
                        string[] vectors = values[j].Split(',');
                        support_planes_points.Add(new Vector3(float.Parse(vectors[0]), float.Parse(vectors[1]), float.Parse(vectors[2])));
                    }

                    support_planes.Add(support_planes_points);
                }

            }
            return (bounding_box, support_planes);
        }

        static LayoutPlanner()
        {
            toolman = new GameObject("toolman").transform;
            pointsLayout = new GameObject("pointsLayout").transform;

            var files = Directory.GetFiles(Params.modelPlane_dir, "*.txt");

            foreach (string file in files)
            {
                //Debug.Log("----->" + file);
                string fname = Path.GetFileName(file);
                string model_id = fname.Replace(".txt", "");
                //Debug.Log("----->" + fname);
                List<Vector3> points = new List<Vector3>();

                if (blackListForPlanes.Contains(model_id)) continue;

                (List<Vector3>, List<List<Vector3>>) vectors = loadPlane(model_id);
                origin_planes.Add(model_id, vectors.Item1);
                origin_support.Add(model_id, vectors.Item2[0]);
            }

        }

        //{"horizonsupport", "vertsupport", "back", "near", "leftside", "onleft", "onright", "rightside", "front", "pairaligned", "pairaround", "under", "oncenter"}
        /*
         * pairelation：
         
         * 接触关系
        'horizonsupport': 水平支撑面相邻，例如挂在墙上
        'vertsupport': 垂直支撑面相邻，例如放在桌上
        
         * 方位
        'onleft'：在左上，例如放在桌面的左边
        'onright'：类似上
        'back'：active在anchor后面
        'front'：active在anchor前面
        'leftside'：左边，anchor在active的左边
        'rightside'：右边，anchor在active的右边
        'under'：active在anchor下方
        
        'near'：附近，前后左右皆可能。处理成left
        'pairaligned'：绑定在一起，如挂饰和房间
        'pairaround'：around的pair形式,相当于near

        * 无信息
        'oncenter'：意义不明，位置关系不明确
        */

        static void rearrangeModelID(SceneSemGraph ssg)
        {
            for (int i = 0; i < ssg.m_graphMetaScene.m_metaModellList.Count; ++i)
            {
                ssg.m_graphMetaScene.m_metaModellList[i].id = i;
            }
        }
        static public SceneSemGraph ProcessLayoutByUnalignedRelation(SceneSemGraph _ssg)
        {
            // get transform matrix of No.i Node
            // ssg.m_graphMetaScene.m_metaModellList[ssg.m_graphNodeToModelListIdMap[i]].transformation

            ssg = _ssg;

            rearrangeModelID(ssg);

            getWorldPlane(ssg);

            // process with all pair_relation
            foreach (var node in ssg.m_nodes)
                if (node.nodeType.Contains("relation") && node.activeNodeList.Count > 0 && node.anchorNodeList.Count > 0 && !node.isAligned)
                {
                    if (node.nodeType.Contains("group"))
                    {
                        // such relation, just set active Aligned, which means they won't be adjusted.
                        foreach (var i in node.activeNodeList)
                        {
                            ssg.m_nodes[i].isAligned = true;
                        }

                    }
                    else
                    {
                        if (!ssg.m_graphNodeToModelListIdMap.ContainsKey(node.activeNodeList[0]) || !ssg.m_graphNodeToModelListIdMap.ContainsKey(node.anchorNodeList[0])) continue;

                        int active_idx = ssg.m_graphNodeToModelListIdMap[node.activeNodeList[0]];
                        int anchor_idx = ssg.m_graphNodeToModelListIdMap[node.anchorNodeList[0]];
                        MetaModel active = ssg.m_graphMetaScene.m_metaModellList[active_idx];
                        MetaModel anchor = ssg.m_graphMetaScene.m_metaModellList[anchor_idx];

                        // special objects....
                        string activeName = ssg.m_nodes[node.activeNodeList[0]].nodeName;
                        if ( (activeName.Contains("tv") || activeName.Contains("speaker") ) )
                        {
                            ssg.m_nodes[node.activeNodeList[0]].isAligned = true;
                            continue;
                        }


                            //A1: collision process for anchor metamodel has no-inplace relation
                            if (!ssg.m_nodes[node.anchorNodeList[0]].isAligned)
                        {
                            adjustForCollision(anchor, ssg, Vector3.zero);
                        }

                        if (!planes.ContainsKey(active_idx) || !planes.ContainsKey(anchor_idx))
                        {
                            Debug.Log("ModelPlane.txt not contains model id, so model position optimize error!");
                            continue;
                        }

                        Vector3 dis;
                        Vector3 pos;
                        float distance;

                        // the direction of active to move for avoid collision
                        Vector3 direct = Vector3.zero;
                        switch (node.nodeName)
                        {
                            case "horizonsupport":
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - zsize(active) - zsize(anchor);
                                if (distance < 0.2f && distance > 0f && HorizonInAnchor(active, anchor))
                                    break;

                                moveMetaWithActives(active, anchor.transformation.GetPosition() + anchor.transformation.GetForward() * (zsize(anchor) / 2 + zsize(active) / 2));
                                break;

                            case "vertsupport":
                                distance = downHeight(active) - supportHeight(anchor);
                                if (distance < 0.2f && distance > 0f && VertInAnchor(active, anchor))
                                    break;
                                moveMetaWithActives(active, anchor.transformation.GetPosition() + anchor.transformation.GetUp() * (dist_support(anchor) + ysize(active) / 2));

                                break;

                            case "onleft":
                                direct = -anchor.transformation.GetRight();
                                distance = downHeight(active) - supportHeight(anchor);
                                if (distance < 0.2f && distance > 0f && leftAnchor(active, anchor))
                                    break;
                                moveMetaWithActives(active,
                                    anchor.transformation.GetPosition() + anchor.transformation.GetUp() * (dist_support(anchor) + ysize(active) / 2) - anchor.transformation.GetRight() * 0.1f
                                    );
                                break;

                            case "onright":
                                direct = anchor.transformation.GetRight();
                                distance = downHeight(active) - supportHeight(anchor);
                                if (distance < 0.2f && distance > 0f && RightAnchor(active, anchor))
                                    break;
                                moveMetaWithActives(active,
                                    anchor.transformation.GetPosition() + anchor.transformation.GetUp() * (dist_support(anchor) + ysize(active) / 2) + anchor.transformation.GetRight() * 0.1f
                                    );
                                break;

                            case "back":
                                direct = -anchor.transformation.GetForward();
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - zsize(active) / 2 - zsize(anchor) / 2;
                                if (Vector3.Angle(dis, -anchor.transformation.GetForward()) <= 45f && distance > 0f && distance < 1.5f) //valid
                                    break;

                                pos = anchor.transformation.GetPosition() - anchor.transformation.GetForward().normalized * (zsize(active) / 2 + zsize(anchor) / 2 + 0.2f);
                                pos.y = active.transformation.GetPosition().y;
                                moveMetaWithActives(active, pos);

                                break;

                            case "front":
                                direct = anchor.transformation.GetForward();
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - zsize(active) / 2 - zsize(anchor) / 2;
                                if (Vector3.Angle(dis, anchor.transformation.GetForward()) <= 45f && distance > 0f && distance < 1f) //valid
                                    break;

                                pos = anchor.transformation.GetPosition() + anchor.transformation.GetForward().normalized * (zsize(active) / 2 + zsize(anchor) / 2 + 0.2f);
                                pos.y = active.transformation.GetPosition().y;
                                moveMetaWithActives(active, pos);

                                break;


                            case "near":
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - maxsize(active) / 2 - maxsize(anchor) / 2;
                                if (distance > 0f && distance < 1.5f)
                                    break;
                                Vector3 pos_active = anchor.transformation.GetPosition() + anchor.transformation.GetForward().normalized * (zsize(active) / 2 + zsize(anchor) / 2 + 0.3f);
                                pos_active.y = active.transformation.GetPosition().y;
                                moveMetaWithActives(active, pos_active);

                                break;

                            case "leftside":
                                direct = anchor.transformation.GetRight();
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - xsize(active) / 2 - xsize(anchor) / 2;
                                if (Vector3.Angle(dis, anchor.transformation.GetRight()) <= 45f && distance > 0f && distance < 1f) //valid
                                    break;

                                pos = anchor.transformation.GetPosition() - anchor.transformation.GetRight().normalized * (xsize(active) / 2 + xsize(anchor) / 2 + 0.2f);
                                pos.y = active.transformation.GetPosition().y;
                                moveMetaWithActives(active, pos);

                                break;

                            case "rightside":
                                direct = -anchor.transformation.GetRight();

                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - xsize(active) / 2 - xsize(anchor) / 2;
                                if (Vector3.Angle(dis, -anchor.transformation.GetRight()) <= 45f && distance > 0f && distance < 1f) //valid
                                    break;

                                pos = anchor.transformation.GetPosition() + anchor.transformation.GetRight().normalized * (xsize(active) / 2 + xsize(anchor) / 2 + 0.2f);
                                pos.y = active.transformation.GetPosition().y;
                                moveMetaWithActives(active, pos);

                                break;


                            // hardly use relation below
                            case "under":
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - ysize(active) / 2 - ysize(anchor) / 2;
                                if (Vector3.Angle(dis, -anchor.transformation.GetUp()) <= 45f && distance > 0f && distance < 1.5f) //valid
                                    break;
                                moveMetaWithActives(active, anchor.transformation.GetPosition().ZeroY() + new Vector3(0, active.transformation.GetPosition().y, 0));

                                break;
                            case "pairaligned":
                                break;
                            case "pairaround":
                                dis = active.transformation.GetPosition() - anchor.transformation.GetPosition();
                                distance = Vector3.Magnitude(dis) - maxsize(active) / 2 - maxsize(anchor) / 2;
                                if (distance > 0f && distance < 1.5f)
                                    break;
                                moveMetaWithActives(active,
                                    anchor.transformation.GetPosition() + anchor.transformation.GetForward().normalized * (maxsize(active) / 2 + maxsize(anchor) / 2 + 0.2f));
                                break;

                            case "oncenter":
                                break;
                        }

                        //A2: collision process form active metamodel has no-inplace relation
                        if (node.nodeName == "vertsupport" || node.nodeName == "onleft" || node.nodeName == "onright")
                            adjustForCollision(active, ssg, Vector3.zero, anchor.id, true);
                        else adjustForCollision(active, ssg, direct, - 1, false);
                    }
                }

            //B: collision process for all unaligned metamodel
            for (int i = 0; i < ssg.m_nodes.Count; ++i)
            {
                if (ssg.m_nodes[i].nodeType == "object" && !ssg.m_nodes[i].isAligned)
                {
                    if (!ssg.m_graphNodeToModelListIdMap.ContainsKey(i))
                    {
                        Debug.Log("graphNodeToModelListIdMap contains no key: " + i);
                        continue;
                    }

                    MetaModel meta = ssg.m_graphMetaScene.m_metaModellList[ssg.m_graphNodeToModelListIdMap[i]];
                    adjustForCollision(meta, ssg,Vector3.zero);
                }
            }

            // draw
            //if (pointsLayout != null) UnityEngine.Object.Destroy(pointsLayout.gameObject);
            //pointsLayout = new GameObject("pointsLayout").transform;
            //foreach (var tmp in planes.Values)
            //{
            //    foreach (var p in tmp)
            //    {
            //        GameObject t = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            //        t.transform.position = p;
            //        t.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
            //        t.GetComponent<Renderer>().material.color = Color.red;
            //        t.transform.parent = pointsLayout;
            //    }
            //}

            //foreach (var tmp in supports.Values)
            //{
            //    foreach (var p in tmp)
            //    {
            //        GameObject t = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            //        t.transform.position = p;
            //        t.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
            //        t.GetComponent<Renderer>().material.color = Color.blue;
            //        t.transform.parent = pointsLayout;
            //    }
            //}

            return ssg;

        }

        static Vector3 randomDirection()
        {
            return new Vector3((float)(Util.GenRandomDouble(-1, 1)), (float)(Util.GenRandomDouble(-1, 1)), (float)(Util.GenRandomDouble(-1, 1))).normalized;
        }

        // makesure the index node has no collision with anything
        static void adjustForCollision(MetaModel meta, SceneSemGraph ssg, Vector3 dir, int anchorIndex = -1, bool isVertSupport = false)
        {
            // robust
            if (!origin_planes.ContainsKey(meta.name))
            {
                Debug.LogError(meta.name + "not exists in MODELPLANES.TXT");
                return;
            }

            // adjust pos to avoid collsion
            // makesure this meta has no collision with anything
            bool isCollide = true;
            for (int cnt = 0; cnt < 100 && isCollide; ++cnt)
            {
                isCollide = false;
                for (int j = 0; j < ssg.m_graphMetaScene.m_metaModellList.Count; ++j)
                {
                    if (meta.id == j) continue;
                    if (anchorIndex != -1) if (anchorIndex == j) continue;

                    var mj = ssg.m_graphMetaScene.m_metaModellList[j];
                    var mi = meta;

                    if (mj.name.Contains("room")) continue;

                    if (metaCollision(mi, mj))
                    {
                        // get a random direction
                        Vector3 direct = randomDirection();

                        // make sure the object move to the right direction
                        if (dir != Vector3.zero)
                            while (Mathf.Abs(Vector3.Angle(dir, direct)) > 90)
                                direct = randomDirection();

                        Vector3 proj = new Vector3(direct.x, 0f, direct.z).normalized;
                        Debug.Log("randomDir: " + direct);

                        // move it
                        for (int cnt2 = 0; cnt2 < 1000 && metaCollision(mi, mj); ++cnt2)
                        {
                            if (!isVertSupport)
                            {
                                moveMetaWithActives(mi, mi.transformation.GetPosition() + proj * 0.05f);

                            }
                            else
                            {
                                MetaModel anchorMeta = ssg.m_graphMetaScene.m_metaModellList[anchorIndex];
                                Vector3 rand_xz = getRandPosInAnchor(anchorMeta);
                                Debug.Log("randomXZ: " + rand_xz);
                                moveMetaWithActives(mi, new Vector3(rand_xz.x, mi.transformation.GetPosition().y, rand_xz.z));
                            }
                        }
                        isCollide = true;
                        break;
                    }
                }
            }

            // set Aligned
            foreach (var pp in ssg.m_graphNodeToModelListIdMap)
            {
                if (pp.Value == meta.id)
                {
                    ssg.m_nodes[pp.Key].isAligned = true;
                    break;
                }
            }
        }

        static void adjustForCollisionByGroup(int index, Vector3 direct, SceneSemGraph ssg, int anchor = -1)
        {

        }

        static void moveMetaWithActives(MetaModel meta, Vector3 pos)
        {
            var zw = ssg.getSupportOrGroupActives(meta.id);

            Vector3 dis = pos - meta.transformation.GetPosition();

            moveMeta(meta, pos);
            foreach (int zww in zw)
            {
                var active = ssg.m_graphMetaScene.m_metaModellList[zww];
                moveMeta(active, active.transformation.GetPosition() + dis);
            }
        }

        static void moveMeta(MetaModel meta, Vector3 pos)
        {
            meta.transformation = Matrix4x4.TRS(pos, meta.transformation.GetRotation(), meta.transformation.GetScale());

            //adjust plane and support data

            toolman.position = meta.transformation.GetPosition();
            toolman.localScale = meta.transformation.GetScale();
            toolman.rotation = meta.transformation.GetRotation();

            List<Vector3> result = new List<Vector3>();
            List<Vector3> result_support = new List<Vector3>();

            var origin = origin_planes[meta.name];
            foreach (var pl in origin)
            {
                Vector3 p = toolman.TransformPoint(pl);

                result.Add(p);
            }
            var support = origin_support[meta.name];
            foreach (var p in support)
            {
                Vector3 v = toolman.TransformPoint(p);
                result_support.Add(v);
            }

            planes[meta.id] = result;
            supports[meta.id] = result_support;
        }

        static bool metaCollision(MetaModel mi, MetaModel mj)
        {
            if (mi.name.Contains("room") || mj.name.Contains("room")) return false;

            if (!planes.ContainsKey(mi.id) || !planes.ContainsKey(mj.id)) return false;

            if (IntersectOBB(mi, mj))
            {
                Debug.Log("Collision!!! " + mi.name + " and " + mj.name);
                return true;
            }
            return false;
        }
        class Box
        {
            public Vector3 center;
            public Quaternion rotation;
            public Vector3 extents;
            public Box(MetaModel m)
            {
                //planes[mi.id][0]
                center = m.transformation.GetPosition();
                rotation = Quaternion.LookRotation(frontPlane(m.id).normal, upPlane(m.id).normal);
                extents = new Vector3(0.5F * xsize(m), 0.5F * ysize(m), 0.5F * zsize(m));
            }
        }
        // untested!
        static bool IntersectOBB(MetaModel mi, MetaModel mj)
        {
            Box box0 = new Box(mi), box1 = new Box(mj);

            Vector3 v = box1.center - box0.center;

            //Compute A's basis
            Vector3 VAx = box0.rotation * new Vector3(1, 0, 0);
            Vector3 VAy = box0.rotation * new Vector3(0, 1, 0);
            Vector3 VAz = box0.rotation * new Vector3(0, 0, 1);

            Vector3[] VA = new Vector3[3];
            VA[0] = VAx;
            VA[1] = VAy;
            VA[2] = VAz;

            //Compute B's basis
            Vector3 VBx = box1.rotation * new Vector3(1, 0, 0);
            Vector3 VBy = box1.rotation * new Vector3(0, 1, 0);
            Vector3 VBz = box1.rotation * new Vector3(0, 0, 1);

            Vector3[] VB = new Vector3[3];
            VB[0] = VBx;
            VB[1] = VBy;
            VB[2] = VBz;

            Vector3 T = new Vector3(Vector3.Dot(v, VAx), Vector3.Dot(v, VAy), Vector3.Dot(v, VAz));

            float[,] R = new float[3, 3];
            float[,] FR = new float[3, 3];
            float ra, rb, t;

            for (int i = 0; i < 3; i++)
            {
                for (int k = 0; k < 3; k++)
                {
                    R[i, k] = Vector3.Dot(VA[i], VB[k]);
                    FR[i, k] = 1e-6f + Mathf.Abs(R[i, k]);
                }
            }

            // A's basis vectors
            for (int i = 0; i < 3; i++)
            {
                ra = box0.extents[i];
                rb = box1.extents[0] * FR[i, 0] + box1.extents[1] * FR[i, 1] + box1.extents[2] * FR[i, 2];
                t = Mathf.Abs(T[i]);
                if (t > ra + rb) return false;
            }

            // B's basis vectors
            for (int k = 0; k < 3; k++)
            {
                ra = box0.extents[0] * FR[0, k] + box0.extents[1] * FR[1, k] + box0.extents[2] * FR[2, k];
                rb = box1.extents[k];
                t = Mathf.Abs(T[0] * R[0, k] + T[1] * R[1, k] + T[2] * R[2, k]);
                if (t > ra + rb) return false;
            }

            //9 cross products

            //L = A0 x B0
            ra = box0.extents[1] * FR[2, 0] + box0.extents[2] * FR[1, 0];
            rb = box1.extents[1] * FR[0, 2] + box1.extents[2] * FR[0, 1];
            t = Mathf.Abs(T[2] * R[1, 0] - T[1] * R[2, 0]);
            if (t > ra + rb) return false;

            //L = A0 x B1
            ra = box0.extents[1] * FR[2, 1] + box0.extents[2] * FR[1, 1];
            rb = box1.extents[0] * FR[0, 2] + box1.extents[2] * FR[0, 0];
            t = Mathf.Abs(T[2] * R[1, 1] - T[1] * R[2, 1]);
            if (t > ra + rb) return false;

            //L = A0 x B2
            ra = box0.extents[1] * FR[2, 2] + box0.extents[2] * FR[1, 2];
            rb = box1.extents[0] * FR[0, 1] + box1.extents[1] * FR[0, 0];
            t = Mathf.Abs(T[2] * R[1, 2] - T[1] * R[2, 2]);
            if (t > ra + rb) return false;

            //L = A1 x B0
            ra = box0.extents[0] * FR[2, 0] + box0.extents[2] * FR[0, 0];
            rb = box1.extents[1] * FR[1, 2] + box1.extents[2] * FR[1, 1];
            t = Mathf.Abs(T[0] * R[2, 0] - T[2] * R[0, 0]);
            if (t > ra + rb) return false;

            //L = A1 x B1
            ra = box0.extents[0] * FR[2, 1] + box0.extents[2] * FR[0, 1];
            rb = box1.extents[0] * FR[1, 2] + box1.extents[2] * FR[1, 0];
            t = Mathf.Abs(T[0] * R[2, 1] - T[2] * R[0, 1]);
            if (t > ra + rb) return false;

            //L = A1 x B2
            ra = box0.extents[0] * FR[2, 2] + box0.extents[2] * FR[0, 2];
            rb = box1.extents[0] * FR[1, 1] + box1.extents[1] * FR[1, 0];
            t = Mathf.Abs(T[0] * R[2, 2] - T[2] * R[0, 2]);
            if (t > ra + rb) return false;

            //L = A2 x B0
            ra = box0.extents[0] * FR[1, 0] + box0.extents[1] * FR[0, 0];
            rb = box1.extents[1] * FR[2, 2] + box1.extents[2] * FR[2, 1];
            t = Mathf.Abs(T[1] * R[0, 0] - T[0] * R[1, 0]);
            if (t > ra + rb) return false;

            //L = A2 x B1
            ra = box0.extents[0] * FR[1, 1] + box0.extents[1] * FR[0, 1];
            rb = box1.extents[0] * FR[2, 2] + box1.extents[2] * FR[2, 0];
            t = Mathf.Abs(T[1] * R[0, 1] - T[0] * R[1, 1]);
            if (t > ra + rb) return false;

            //L = A2 x B2
            ra = box0.extents[0] * FR[1, 2] + box0.extents[1] * FR[0, 2];
            rb = box1.extents[0] * FR[2, 1] + box1.extents[1] * FR[2, 0];
            t = Mathf.Abs(T[1] * R[0, 2] - T[0] * R[1, 2]);
            if (t > ra + rb) return false;

            return true;
        }

        public static Dictionary<string, List<Vector3>> str_planes;
        //transform local to global
        //static void getWorldPlane(SceneSemGraph ssg)
        //{
        //    str_planes = new Dictionary<string, List<Vector3>>();
        //    foreach (var meta in ssg.m_graphMetaScene.m_metaModellList)
        //    {
        //        toolman.position = meta.transformation.GetPosition();
        //        toolman.localScale = meta.transformation.GetScale();
        //        toolman.rotation = meta.transformation.GetRotation();

        //        if (!origin_planes.ContainsKey(meta.name))
        //        {
        //            Debug.Log(meta.name + "not exists in MODELPLANES.TXT");
        //            continue;
        //        }

        //        List<Vector3> result = new List<Vector3>();
        //        var origin = origin_planes[meta.name];
        //        foreach (var pl in origin)
        //        {
        //            Vector3 p = toolman.TransformPoint(pl);

        //            result.Add(p);

        //            //GameObject t = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        //            //t.transform.position = p;
        //            //t.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
        //            //t.transform.parent = pointsLayout;
        //        }
        //        str_planes[meta.name] = result;

        //    }
        //}

        public static Dictionary<int, List<Vector3>> planes = new Dictionary<int, List<Vector3>>();
        public static Dictionary<int, List<Vector3>> supports = new Dictionary<int, List<Vector3>>();
        static void getWorldPlane(SceneSemGraph ssg)
        {
            planes = new Dictionary<int, List<Vector3>>();
            for (int i = 0; i < ssg.m_graphMetaScene.m_metaModellList.Count; i++)
            {
                var meta = ssg.m_graphMetaScene.m_metaModellList[i];
                toolman.position = meta.transformation.GetPosition();
                toolman.localScale = meta.transformation.GetScale();
                toolman.rotation = meta.transformation.GetRotation();
                if (!origin_planes.ContainsKey(meta.name))
                {
                    Debug.Log(meta.name + "not exists in MODELPLANES.TXT");
                    continue;
                }

                List<Vector3> result = new List<Vector3>();
                List<Vector3> result_support = new List<Vector3>();

                var origin = origin_planes[meta.name];
                foreach (var pl in origin)
                {
                    Vector3 p = toolman.TransformPoint(pl);

                    result.Add(p);
                }
                var support = origin_support[meta.name];
                foreach (var p in support)
                {
                    Vector3 v = toolman.TransformPoint(p);
                    result_support.Add(v);
                }

                planes[meta.id] = result;
                supports[meta.id] = result_support;
            }
        }

        // get plane of bound box
        static public Plane frontPlane(int model_id)
        {
            List<Vector3> points = planes[model_id];

            return new Plane(points[1], points[2], points[5]);
        }
        static public Plane rightPlane(int model_id)
        {
            List<Vector3> points = planes[model_id];
            return new Plane(points[0], points[1], points[5]);
        }
        static public Plane upPlane(int model_id)
        {
            List<Vector3> points = planes[model_id];
            return new Plane(points[0], points[1], points[2]);
        }
        // get support plane
        static public Plane upsupport(int model_id)
        {
            List<Vector3> points = supports[model_id];
            return new Plane(points[0], points[1], points[2]);
        }

        // get size of bound box
        static public float xsize(MetaModel model)
        {
            Plane right = rightPlane(model.id);
            return Math.Abs(right.GetDistanceToPoint(model.transformation.GetPosition())) * 2;
        }
        static public float ysize(MetaModel model)
        {
            Plane up = upPlane(model.id);
            return Math.Abs(up.GetDistanceToPoint(model.transformation.GetPosition())) * 2;
        }
        static public float zsize(MetaModel model)
        {
            Plane front = frontPlane(model.id);
            return Math.Abs(front.GetDistanceToPoint(model.transformation.GetPosition())) * 2;
        }
        static public float maxsize(MetaModel model)
        {
            return Math.Max(xsize(model), zsize(model));
        }

        // get distance from center to support plane
        static public float dist_support(MetaModel model)
        {
            Plane upsupport_plane = upsupport(model.id);
            return Math.Abs(upsupport_plane.GetDistanceToPoint(model.transformation.GetPosition()));
        }

        // get highest height and lowest height
        static public float upHeight(MetaModel model)
        {
            List<Vector3> points = planes[model.id];
            return Math.Max(Math.Max(Math.Max(points[0].y, points[1].y), points[2].y), points[3].y);
        }
        static public float downHeight(MetaModel model)
        {
            List<Vector3> points = planes[model.id];
            return Math.Min(Math.Min(Math.Min(points[4].y, points[5].y), points[6].y), points[7].y);
        }
        static public float supportHeight(MetaModel model)
        {
            List<Vector3> points = supports[model.id];
            return Math.Max(Math.Max(Math.Max(points[0].y, points[1].y), points[2].y), points[3].y);
        }

        // judge whether active's center lies in anchor
        static public bool inSpace(float x, float z, MetaModel anchor)
        {
            List<Vector3> anchor_support = supports[anchor.id];
            if (anchor_support.Count != 4)
                Debug.LogError("loal modelplanes format error, or modelplanes file format error");
            Vector3 A = anchor_support[0], B = anchor_support[1], C = anchor_support[2], D = anchor_support[3];

            float a = (B.x - A.x) * (z - A.z) - (B.z - A.z) * (x - A.x);
            float b = (C.x - B.x) * (z - B.z) - (C.z - B.z) * (x - B.x);
            float c = (D.x - C.x) * (z - C.z) - (D.z - C.z) * (x - C.x);
            float d = (A.x - D.x) * (z - D.z) - (A.z - D.z) * (x - D.x);
            if ((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0))
                return true;

            return false;
        }
        static public bool VertInAnchor(MetaModel active, MetaModel anchor)
        {
            Vector3 pos_active = active.transformation.GetPosition();
            Vector3 pos_anchor = anchor.transformation.GetPosition();
            List<Vector3> anchor_support = supports[anchor.id];

            float x = pos_active.x, z = pos_active.z;
            return inSpace(x, z, anchor);
        }
        static public bool HorizonInAnchor(MetaModel active, MetaModel anchor)
        {
            Vector3 pos_active = active.transformation.GetPosition();
            Vector3 pos_anchor = anchor.transformation.GetPosition();
            List<Vector3> anchor_boundingbox = planes[anchor.id];

            float xmin = 100f, ymin = 100f, xmax = -100f, ymax = -100f;
            foreach (Vector3 point in anchor_boundingbox)
            {
                if (point.x < xmin) xmin = point.x;
                if (point.x > xmax) xmax = point.x;
                if (point.y < ymin) ymin = point.y;
                if (point.y > ymax) ymax = point.y;
            }

            if (pos_active.x > xmin && pos_active.x < xmax && pos_active.y > ymin && pos_active.y < ymax)
                return true;
            return false;
        }
        static public bool leftAnchor(MetaModel active, MetaModel anchor)
        {
            if (active.transformation.GetPosition().x <= anchor.transformation.GetPosition().x)
                return false;
            return VertInAnchor(active, anchor);
        }
        static public bool RightAnchor(MetaModel active, MetaModel anchor)
        {
            if (active.transformation.GetPosition().x >= anchor.transformation.GetPosition().x)
                return false;
            return VertInAnchor(active, anchor);

        }

        // get a random (x,z) in anchor
        static public Vector3 getRandPosInAnchor(MetaModel anchor)
        {
            // how much space on the edge of anchor can not put
            float boundCoff = 0.1f;

            List<Vector3> anchor_support = supports[anchor.id];
            float xmin = 1e8f, zmin = 1e8f, xmax = -1e8f, zmax = -1e8f;
            foreach (var point in anchor_support)
            {
                if (point.x < xmin) xmin = point.x;
                if (point.x > xmax) xmax = point.x;
                if (point.z < zmin) zmin = point.z;
                if (point.z > zmax) zmax = point.z;
            }

            float x, z;
            //x = (float)rd.NextDouble() * (xmax - xmin) * (1 - boundCoff) + xmin + (xmax - xmin) * boundCoff;
            //z = (float)rd.NextDouble() * (zmax - zmin) * (1 - boundCoff) + zmin + (zmax - zmin) * boundCoff;
            x = (float)Util.GenRandomDouble(xmin + (xmax - xmin) * boundCoff, xmax - (xmax - xmin) * boundCoff);
            z = (float)Util.GenRandomDouble(zmin + (zmax - zmin) * boundCoff, zmax - (zmax - zmin) * boundCoff);
            for (int i = 0; i < 1000 && !inSpace(x, z, anchor); i++)
            {
                x = (float)Util.GenRandomDouble(xmin + (xmax - xmin) * boundCoff, xmax - (xmax - xmin) * boundCoff);
                z = (float)Util.GenRandomDouble(zmin + (zmax - zmin) * boundCoff, zmax - (zmax - zmin) * boundCoff);
            }

            return new Vector3(x, 0f, z);
        }
        static public Vector3 getRandPosInSquareAnchor(MetaModel anchor)
        {
            Vector3 pos = anchor.transformation.GetPosition();
            //float xlen =  (float)rd.NextDouble() * 6 - 3;
            //float zlen = (float)rd.NextDouble() * 6 - 3;
            float xlen = (float)Util.GenRandomDouble(3, 6);
            float zlen = (float)Util.GenRandomDouble(3, 6);

            return new Vector3(pos.x + xlen, 0f, pos.z + zlen);
        }
    }
}
